/***************************************************************************
 * @version   --> v1.0
 * @date      -->2021-04-18
 * @brief     -->姿态管理模块
 *            1.通过串口接收上位机发送的指令，主要控制四个飞轮和三轴磁力矩器，
 *              从而调节姿态，同时把当前立方星的姿态信息通过串口发送给上位机
 *            2.通过Arduino串口调试时，可以调节飞轮转速和控制磁力矩器产生的力矩
 *@update     -->2021-05-12
 **************************************************************************/

/*Includes --------------------------------------------------------------*/
#include <ros.h>
#include <learning_test/ADCS.h>
#include <learning_test/sensor.h>
#include <std_msgs/UINT16.h>
#include <Wire.h>       // IIC通信
#include <SCoop.h>      // 使用SCoop线程库
#include <string.h>     // 使用内存拷贝
#include "IMUGY85.h"    // 姿态敏感器
#include <MsTimer2.h>    // 使用定时器

/* Private define ------------------------------------------------------*/
// 引脚定义
const int WHEEL_DIR0 = 53;   // 1号电机方向控制引脚 53
const int WHEEL_FG0 = 3;    // 1号电机霍尔信号引脚 3，对应内部中断5
const int WHEEL_PWM0 = 9;   // 1号电机转速控制引脚 9
const int WHEEL_DIR1 = 22;  // 2号电机方向控制引脚 22
const int WHEEL_FG1 = 2;   // 2号电机霍尔信号引脚 A8，对应内部中断4
const int WHEEL_PWM1 = 5;   // 2号电机转速控制引脚 5
const int WHEEL_DIR2 = 40;  // 3号电机方向控制引脚 40
const int WHEEL_FG2 = 18;   // 3号电机霍尔信号引脚 18，对应内部中断3
const int WHEEL_PWM2 = 44;  // 3号电机转速控制引脚 44
const int WHEEL_DIR3 = 42;  // 4号电机方向控制引脚 42
const int WHEEL_FG3 = 19;   // 4号电机霍尔信号引脚 19，内部中断2
const int WHEEL_PWM3 = 45;  // 4号电机方向控制引脚 45

const int MAG_IN0 = 23;    // A磁矩控制引脚 23
const int MAG_IN1 = 24;    // A磁矩控制引脚 24
const int MAG_PWMA = 6;     // A磁矩使能引脚 6
const int MAG_IN2 = 25;    // B磁矩控制引脚 25
const int MAG_IN3 = 26;    // B磁矩控制引脚 26
const int MAG_PWMB = 7;     // B磁矩使能引脚 7
const int MAG_IN4 = 27;    // C磁矩控制引脚 27
const int MAG_IN5 = 28;    // C磁矩控制引脚 28
const int MAG_PWMC = 8;     // C磁矩控制引脚 8

IMUGY85 imu;      // IMU（九轴）传感器

// 电池电量记录
volatile uint8_t power_level = 9;
int volt;

// ISR（中断服务程序）中所涉及的变量需要被声明为volatile易变变量
// 状态标志量，飞轮控制，磁力矩器
volatile bool wheel_on = false;
volatile bool magnet_on = false;

// 飞轮转速记录
volatile int16_t speed_wheel1_measurement = 0;  // 飞轮1的转速测量值，-32768~32767
volatile int8_t speed_wheel1_setting = 0;      // 飞轮1的转速设定值，0-100
volatile bool wheel1_forward = true;             // 飞轮1正转，规定顺为正逆为负
uint16_t wheel1_counter = 0;                    // 飞轮1编码计数器
volatile int16_t speed_wheel2_measurement = 0;  // 飞轮2的转速测量值，-32768~32767
volatile int8_t speed_wheel2_setting = 0;      // 飞轮2的转速设定值，
volatile bool wheel2_forward = true;             // 飞轮2正转
uint16_t wheel2_counter = 0;                    // 飞轮2编码计数器
volatile int16_t speed_wheel3_measurement = 0;  // 飞轮3的转速测量值，-32768~32767
volatile int8_t speed_wheel3_setting = 0;      // 飞轮3的转速设定值，
volatile bool wheel3_forward = true;             // 飞轮3正转
uint16_t wheel3_counter = 0;                    // 飞轮3编码计数器
volatile int16_t speed_wheel4_measurement = 0;  // 飞轮4的转速测量值，-32768~32767
volatile int8_t speed_wheel4_setting = 0;      // 飞轮4的转速设定值，
volatile bool wheel4_forward = true;             // 飞轮4正转
uint16_t wheel4_counter = 0;                    // 飞轮4编码计数器

// 磁矩设置值
volatile uint8_t x_magnetmoment_setting = 0;
volatile uint8_t y_magnetmoment_setting = 0;
volatile uint8_t z_magnetmoment_setting = 0;
volatile bool x_magnet_forward = true;    // X轴磁矩正向
volatile bool y_magnet_forward = true;    // Y轴磁矩正向      
volatile bool z_magnet_forward = true;    // Z轴磁矩正向

// IMU数据
float eulers[3] = {0};
float angular_velocities[3] = {0};
float accelerations[3] = {0};

ros::NodeHandle nh;
std_msgs::UInt16 power_msg;
std_msgs::UInt16 wheel_msg;
std_msgs::UInt16 magnet_msg;
learning_test::ADCS measurement_msg;
learning_test::sensor wheel1_msg;
learning_test::sensor wheel2_msg;
learning_test::sensor wheel3_msg;
learning_test::sensor wheel4_msg;
learning_test::sensor x_magnet_msg;
learning_test::sensor y_magnet_msg;
learning_test::sensor z_magnet_msg;

// 输入0-100，设定飞轮一的转速
void subscriberCallback0(const learning_test::sensor& wheel1_setting){
  wheel1_forward = wheel1_setting.forward;
  speed_wheel1_setting = (wheel1_setting.speed) * 10;
  if(wheel1_forward){
    digitalWrite(WHEEL_DIR0, HIGH);
  }else{digitalWrite(WHEEL_DIR0, LOW);}
}

// 输入0-100，设定飞轮二的转速
void subscriberCallback1(const learning_test::sensor& wheel2_setting){
  wheel2_forward = wheel2_setting.forward;
  speed_wheel2_setting = (wheel2_setting.speed) * 255 / 100;
  if(wheel2_forward){
    digitalWrite(WHEEL_DIR1, HIGH);
  }else{digitalWrite(WHEEL_DIR1, LOW);}
}

// 输入0-100，设定飞轮三的转速
void subscriberCallback2(const learning_test::sensor& wheel3_setting){
  wheel3_forward = wheel3_setting.forward;
  speed_wheel3_setting = (wheel3_setting.speed) * 255 / 100;
  if(wheel3_forward){
    digitalWrite(WHEEL_DIR2, HIGH);
  }else{digitalWrite(WHEEL_DIR2, LOW);}
}

// 输入0-100，设定飞轮四的转速
void subscriberCallback3(const learning_test::sensor& wheel4_setting){
  wheel4_forward = wheel4_setting.forward;
  speed_wheel4_setting = (wheel4_setting.speed)* 255 / 100;
  if(wheel4_forward){
    digitalWrite(WHEEL_DIR3, HIGH);
  }else{digitalWrite(WHEEL_DIR3, LOW);}
}

// 输入0-100，设定x轴的磁矩值
void subscriberCallback4(const learning_test::sensor& x_magnet_setting){
  x_magnet_forward = x_magnet_setting.forward;
  x_magnetmoment_setting = (x_magnet_setting.speed) * 255 / 100;
  if(x_magnet_forward){
    digitalWrite(MAG_IN0, LOW);
    digitalWrite(MAG_IN1, HIGH);
  }else{
    digitalWrite(MAG_IN0, HIGH);
    digitalWrite(MAG_IN1, LOW);
  }
}

// 输入0-100，设定y轴的磁矩值
void subscriberCallback5(const learning_test::sensor& y_magnet_setting){
  y_magnet_forward = y_magnet_setting.forward;
  y_magnetmoment_setting = (y_magnet_setting.speed) * 255 / 100;
  if(y_magnet_forward){
    digitalWrite(MAG_IN2, LOW);
    digitalWrite(MAG_IN3, HIGH);
  }else{
    digitalWrite(MAG_IN2, HIGH);
    digitalWrite(MAG_IN3, LOW);
  }
}

// 输入0-100，设定z轴的磁矩值
void subscriberCallback6(const learning_test::sensor& z_magnet_setting){
  z_magnet_forward = z_magnet_setting.forward;
  z_magnetmoment_setting = (z_magnet_setting.speed) * 255 / 100;
  if(z_magnet_forward){
    digitalWrite(MAG_IN4, LOW);
    digitalWrite(MAG_IN5, HIGH);
  }else{
    digitalWrite(MAG_IN4, HIGH);
    digitalWrite(MAG_IN5, LOW);
  }
}

void subscriberCallback7(const std_msgs::UInt16& wheel_sw){
  // 是否打开飞轮调姿
  if(wheel_sw.data == 1){
    wheel_on = true;
  }else{wheel_on = false;}
}

void subscriberCallback8(const std_msgs::UInt16& magnet_sw){
  // 是否打开三轴磁力矩器调姿
  if(magnet_sw.data == 1){
    magnet_on = true;
  }else{magnet_on = false;}
}

ros::Publisher ADCS_publisher("ADCS_state", &measurement_msg);
ros::Publisher power_publisher("power_level", &power_msg);
ros::Subscriber<learning_test::sensor> wheel1_subscriber("wheel1_setting", &subscriberCallback0);
ros::Subscriber<learning_test::sensor> wheel2_subscriber("wheel2_setting", &subscriberCallback1);
ros::Subscriber<learning_test::sensor> wheel3_subscriber("wheel3_setting", &subscriberCallback2);
ros::Subscriber<learning_test::sensor> wheel4_subscriber("wheel4_setting", &subscriberCallback3);
ros::Subscriber<learning_test::sensor> x_magnet_subscriber("x_magnet_setting", &subscriberCallback4);
ros::Subscriber<learning_test::sensor> y_magnet_subscriber("y_magnet_setting", &subscriberCallback5);
ros::Subscriber<learning_test::sensor> z_magnet_subscriber("z_magnet_setting", &subscriberCallback6);
ros::Subscriber<std_msgs::UInt16> wheel_subscriber("wheel_sw", &subscriberCallback7);
ros::Subscriber<std_msgs::UInt16> magnet_subscriber("magnet_sw", &subscriberCallback8);
// 速度转换函数
int16_t measurement(uint16_t wheel_counter, bool wheel_forward){
  int16_t speed_wheel_measurement = wheel_counter/9/0.5*60;
  if(!wheel_forward){
    speed_wheel_measurement = -speed_wheel_measurement;
  }
  return speed_wheel_measurement;
}

// 中断函数
void counter1(){wheel1_counter++;}
void counter2(){wheel2_counter++;}
void counter3(){wheel3_counter++;}
void counter4(){wheel4_counter++;}

// 计时器函数
void pertime(){
  speed_wheel1_measurement = measurement(wheel1_counter, wheel1_forward);
  speed_wheel2_measurement = measurement(wheel2_counter, wheel2_forward);
  speed_wheel3_measurement = measurement(wheel3_counter, wheel3_forward);
  speed_wheel4_measurement = measurement(wheel4_counter, wheel4_forward);
  wheel1_counter = 0;
  wheel2_counter = 0;
  wheel3_counter = 0;
  wheel4_counter = 0;
} 
  
void setup() {
  imu.init();
  Serial.begin(57600);
  delay(20);
  // 飞轮
  pinMode(WHEEL_DIR0, OUTPUT);
  pinMode(WHEEL_DIR1, OUTPUT);
  pinMode(WHEEL_DIR2, OUTPUT);
  pinMode(WHEEL_DIR3, OUTPUT);
  pinMode(WHEEL_FG0, INPUT);
  pinMode(WHEEL_FG1, INPUT);
  pinMode(WHEEL_FG2, INPUT);
  pinMode(WHEEL_FG3, INPUT);
  pinMode(WHEEL_PWM0, OUTPUT);
  pinMode(WHEEL_PWM1, OUTPUT);
  pinMode(WHEEL_PWM2, OUTPUT);
  pinMode(WHEEL_PWM3, OUTPUT);

  // 磁力矩器
  pinMode(MAG_IN0, OUTPUT);
  pinMode(MAG_IN1, OUTPUT);
  pinMode(MAG_IN2, OUTPUT);
  pinMode(MAG_IN3, OUTPUT);
  pinMode(MAG_IN4, OUTPUT);
  pinMode(MAG_IN5, OUTPUT);
  pinMode(MAG_PWMA, OUTPUT);
  pinMode(MAG_PWMB, OUTPUT);
  pinMode(MAG_PWMC, OUTPUT);

  // 外部中断，上升沿触发
  attachInterrupt(5, counter1, RISING);
  attachInterrupt(4, counter2, RISING);
  attachInterrupt(3, counter3, RISING);
  attachInterrupt(2, counter4, RISING);
  // 定时器中断
  MsTimer2::set(500, pertime);    // 定时器定时500ms
  MsTimer2::start();              // 开始计时

  nh.initNode();
  nh.advertise(ADCS_publisher);
  nh.advertise(power_publisher);
  nh.subscribe(wheel1_subscriber);
  nh.subscribe(wheel2_subscriber);
  nh.subscribe(wheel3_subscriber);
  nh.subscribe(wheel4_subscriber);
  nh.subscribe(x_magnet_subscriber);
  nh.subscribe(y_magnet_subscriber);
  nh.subscribe(z_magnet_subscriber);
  nh.subscribe(wheel_subscriber);
  nh.subscribe(magnet_subscriber);
}

void loop() {
  // IMU部分
  imu.update();
  double temp1,temp2, temp3;
  imu.getEuler(&temp1, &temp2, &temp3);
  eulers[0] = temp1;
  eulers[1] = temp2;
  eulers[2] = temp3;
  imu.getGyro(&temp1, &temp2, &temp3);
  angular_velocities[0] = temp1;
  angular_velocities[1] = temp2;
  angular_velocities[2] = temp3;
  imu.getAcceleration(&temp1, &temp2, &temp3);
  accelerations[0] = temp1;
  accelerations[1] = temp2;
  accelerations[2] = temp3;

  // 姿态调节
  if(wheel_on){
    // 电机一直接输出PWM没法驱动，所以通过另一种方法实现
    //analogWrite(WHEEL_PWM0, speed_wheel1_setting);
    digitalWrite(WHEEL_PWM0, HIGH);
    delayMicroseconds(speed_wheel1_setting);        // Approximately 30% duty cycle @ 1KHz
    digitalWrite(WHEEL_PWM0, LOW);
    delayMicroseconds(1000 - speed_wheel1_setting); //修改这里的1000可以调整频率
    analogWrite(WHEEL_PWM1, speed_wheel2_setting);
    analogWrite(WHEEL_PWM2, speed_wheel3_setting);
    analogWrite(WHEEL_PWM3, speed_wheel4_setting);
  }
  else{
    analogWrite(WHEEL_PWM0, 0);
    analogWrite(WHEEL_PWM1, 0);
    analogWrite(WHEEL_PWM2, 0);
    analogWrite(WHEEL_PWM3, 0);
  }
  // 磁棒调节
  if(magnet_on){
    analogWrite(MAG_PWMA, x_magnetmoment_setting);
    analogWrite(MAG_PWMB, y_magnetmoment_setting);
    analogWrite(MAG_PWMC, z_magnetmoment_setting);
  }
  else{
    analogWrite(MAG_PWMA, 0);
    analogWrite(MAG_PWMB, 0);
    analogWrite(MAG_PWMC, 0);
  }
   // 电池电量读取
  volt = analogRead(A9);  // 读取电量引脚的模拟值，原理图为A9脚
  power_level = map(volt, 0, 1023, 0, 10);    // 将读的数据映射到0-10的int
  Serial.print("此时电池电量：");
  Serial.println(power_level);
  measurement_msg.wheel1_speed = speed_wheel1_measurement;
  measurement_msg.wheel1_forward = wheel1_forward;
  measurement_msg.wheel2_speed = speed_wheel2_measurement;
  measurement_msg.wheel2_forward = wheel2_forward;
  measurement_msg.wheel3_speed = speed_wheel3_measurement;
  measurement_msg.wheel3_forward = wheel3_forward;
  measurement_msg.wheel4_speed = speed_wheel4_measurement;
  measurement_msg.wheel4_forward = wheel4_forward;
  measurement_msg.x_mag = x_magnetmoment_setting;
  measurement_msg.x_mag_forward = x_magnet_forward;
  measurement_msg.y_mag = y_magnetmoment_setting;
  measurement_msg.y_mag_forward = y_magnet_forward;
  measurement_msg.z_mag = z_magnetmoment_setting;
  measurement_msg.z_mag_forward = z_magnet_forward;
  measurement_msg.x_euler = eulers[0];
  measurement_msg.y_euler = eulers[1];
  measurement_msg.z_euler = eulers[2];
  measurement_msg.x_angular_velocity = angular_velocities[0];
  measurement_msg.y_angular_velocity = angular_velocities[1];
  measurement_msg.z_angular_velocity = angular_velocities[2];
  measurement_msg.x_acceleration = accelerations[0];
  measurement_msg.y_acceleration = accelerations[0];
  measurement_msg.z_acceleration = accelerations[0];
  power_msg.data = power_level;
  ADCS_publisher.publish(&measurement_msg);
  power_publisher.publish(&power_msg);
  nh.spinOnce();
  delay(20);
}
